#include "MeshMaker.h"
#include <QStack>
#include <QList>
#include <qgl.h>
#include <assert.h>

#define ERROR_OFF 0.0
#define POINTS_PER_RING 10
#define SCALE 1.0

using std::endl;
using std::cout;
using std::vector;


struct Line{
    double3 A, B;
    unsigned char   RA, RB;
    int IndA, IndB;
};

double randf(){
    return (double)rand()/(double)RAND_MAX;
}


void convexHull(std::vector<ControlDisc>* list, TreeMesh* mesh)
{
    int num_points = 0;
    for(int i = 0; i < list->size(); i++)
        num_points += list->at(i).points->size();



    double3           points[num_points], norms[num_points];
    unsigned char    num_norms[num_points];
    unsigned char    list_id[num_points];
    bool             isUniqueLine[num_points * num_points];

    const int mesh_ind = mesh->m_verts->size();

    int ind = 0;

    double dot;

    for(int i = 0; i < list->size(); i++){
        for(int j = 0; j < list->at(i).points->size(); j++){
            points[ind] = list->at(i).points->at(j);
            mesh->m_verts->push_back(points[ind]);
            list_id[ind] = (unsigned char) i;
            isUniqueLine[ind] = true;
            norms[ind] = double3(0.f,0.f,0.f);
            num_norms[ind] = 0;
            ind++;
        }
    }

    while(ind < num_points * num_points){
        isUniqueLine[ind] = true;
        ind++;
    }

    double3 norm, disp;
    ind = 0;

    int bestVert = 0;


    double dist = -1.f;
    double mag;
    for(int i = 0; i < num_points; i++){
        mag = points[i].getMagnitude2();
        if(mag > dist){
            dist = mag;
            bestVert = i;
        }
    }

    double3 C, AC, AB;
    Line line;
    line.A  = points[bestVert];
    line.RA = list_id[bestVert];
    line.IndA = bestVert;

    double3 center = (list->at(line.RA).plane.point - line.A).getNormalized();

    dot = 10.f;
    for(int k = 0; k < num_points; k++){
        if(list_id[k] != line.RA){
             mag = (points[k] - line.A).getNormalized().dot(center);
             if(mag < dot){
                 dot = mag;
                 bestVert = k;
             }
        }
    }

    line.B = points[bestVert];
    line.RB = list_id[bestVert];
    line.IndB = bestVert;

    //cout << center << " and " << line.A << " and " << line.B << endl;


    QStack<Line>* lines = new QStack<Line>();

    Line line2;

    line2.A = line.B;
    line2.B = line.A;
    line2.IndA = line.IndB;
    line2.IndB = line.IndA;
    line2.RA = line.RB;
    line2.RB = line.RA;

    lines->push(line);
    lines->push(line2);

    int count = 0;
    //mesh->m_tris->push_back(Tri(mesh_ind + line.IndA, mesh_ind + line.IndB, -1));

    while(!lines->isEmpty()){
        line = lines->pop();
        if(isUniqueLine[line.IndA * num_points + line.IndB]){
            isUniqueLine[line.IndA * num_points + line.IndB] = false;
            bestVert = -1;


            AB = (line.B - line.A);
            do{
                bestVert++;
                C = points[bestVert];
                AC = (C - line.A);
                norm = AB.cross(AC);
            } while(bestVert == line.IndA || bestVert == line.IndB);


            double test;
            double dot2;
            //cout << "looking for new C" << endl;
            for(int k = 0; k < num_points; k++){
                 if(k != line.IndA && k != line.IndB){
                    disp = (points[k] - line.A);
                    dot2 = disp.dot(norm);
                    test = ((line.A - points[k])).dot(norm) / norm.dot(norm);
                    //cout << k << ": " << test << endl;
                    //if(test * dot2 > 0.0) cout << test << " and " << dot2 << endl;
                    if(test < 0.0){
                        bestVert = k;
                        C = points[k];
                        AC = disp;
                        norm = AB.cross(AC);
                    }
                }
            }

            //cout << "Took: " << bestVert << endl;

            if(true){

                Line LCB, LAC, LBA;

                LCB.A     = C;
                LCB.B     = line.B;
                LCB.RA    = list_id[bestVert];
                LCB.RB    = line.RB;
                LCB.IndA  = bestVert;
                LCB.IndB  = line.IndB;

                LAC.A    = line.A;
                LAC.B    = C;
                LAC.RA   = line.RA;
                LAC.RB   = list_id[bestVert];
                LAC.IndA = line.IndA;
                LAC.IndB = bestVert;

                LBA.A    = line.B;
                LBA.B    = line.A;
                LBA.IndA = line.IndB;
                LBA.IndB = line.IndA;
                LBA.RA   = line.RB;
                LBA.RB   = line.RA;

                ind = LCB.IndA * num_points + LCB.IndB;
                if(isUniqueLine[ind])
                    lines->push(LCB);
                //isUniqueLine[ind] = false;

                ind = LAC.IndA * num_points + LAC.IndB;
                if(isUniqueLine[ind])
                    lines->push(LAC);
                //isUniqueLine[ind] = false;

                ind = LBA.IndA * num_points + LBA.IndB;
                if(isUniqueLine[ind])
                    lines->push(LBA);
               // isUniqueLine[ind] = false;


                if((line.RA != list_id[bestVert] || line.RB != list_id[bestVert])){
                    isUniqueLine[bestVert  * num_points + line.IndA]  = false;
                    isUniqueLine[line.IndB * num_points + bestVert]   = false;

                    mesh->m_tris->push_back(Tri(mesh_ind + line.IndA,
                                                mesh_ind + line.IndB,
                                                mesh_ind + bestVert));//mesh_ind + startInd + (((ind - startInd) + 1) % ring->points->size())));

                    norm.normalize();

                    norms[line.IndA] += norm;
                    norms[line.IndB] += norm;
                    norms[bestVert ] += norm;

                    num_norms[line.IndA]++;
                    num_norms[line.IndB]++;
                    num_norms[bestVert ]++;
                    count++;
                }
           }
        }
    }

    for(int i = 0; i < num_points; i++)
        mesh->m_norms->push_back((norms[i] / (double) num_norms[i]).getNormalized());


    delete lines;

}


void addMultipleRingsToMesh(std::vector<ControlRing>* list, TreeMesh* mesh)
{
    int num_points = 0;
    for(int i = 0; i < list->size(); i++)
        num_points += list->at(i).points->size();



    double3          points[num_points], norms[num_points], tangents[num_points];
    unsigned char    num_norms[num_points];
    unsigned char    list_id[num_points];
    bool             isUniqueLine[num_points * num_points];

    const int mesh_ind = mesh->m_verts->size();

    int ind = 0;

    double dot;
    assert(list->size() != 0);
    Bud *curBud = list->at(0).associated_bud;
    for(int i = 0; i < list->size(); i++){
        for(int j = 0; j < list->at(i).points->size(); j++){
            points[ind] = list->at(i).points->at(j);
            mesh->m_verts->push_back(points[ind]);
            tangents[ind] = list->at(i).tangents->at(j);
            mesh->m_tangents->push_back(tangents[ind]);
            if (list->size() == 2){//cylinder
                if(i == 0){//bottom ring
                    mesh->m_texCoords->push_back(float2(((float) j)/list->at(i).points->size(), 1.0f ));
                }
                else{
                    mesh->m_texCoords->push_back(float2(((float) j)/list->at(i).points->size(), 0.5f ));
                }
            }
            else if (list->size() == 3){//joint
                if(i == 0){//bottom ring
                    mesh->m_texCoords->push_back(float2(((float) j)/list->at(i).points->size(), 0.5f ));
                }
                else{
                    mesh->m_texCoords->push_back(float2(((float) j)/list->at(i).points->size(), 0.0f ));
                }
            }
            list_id[ind] = (unsigned char) i;
            isUniqueLine[ind] = true;
            norms[ind] = double3(0.f,0.f,0.f);
            num_norms[ind] = 0;
            ind++;
        }
    }
    //mesh->num_vertices.push_back(ind);
    mesh->m_buds->push_back(curBud);

    while(ind < num_points * num_points){
        isUniqueLine[ind] = true;
        ind++;
    }

    double3 norm, disp;
    ind = 0;

    int bestVert = 0;


    double dist = -1.f;
    double mag;
    for(int i = 0; i < num_points; i++){
        mag = points[i].getMagnitude2();
        if(mag > dist){
            dist = mag;
            bestVert = i;
        }
    }

    double3 C, AC, AB;
    Line line;
    line.A  = points[bestVert];
    line.RA = list_id[bestVert];
    line.IndA = bestVert;

    double3 center = (list->at(line.RA).center - line.A).getNormalized();

    dot = 10.f;
    for(int k = 0; k < num_points; k++){
        if(list_id[k] != line.RA){
             mag = (points[k] - line.A).getNormalized().dot(center);
             if(mag < dot){
                 dot = mag;
                 bestVert = k;
             }
        }
    }

    line.B = points[bestVert];
    line.RB = list_id[bestVert];
    line.IndB = bestVert;

    //cout << center << " and " << line.A << " and " << line.B << endl;


    QStack<Line>* lines = new QStack<Line>();

    Line line2;

    line2.A = line.B;
    line2.B = line.A;
    line2.IndA = line.IndB;
    line2.IndB = line.IndA;
    line2.RA = line.RB;
    line2.RB = line.RA;

    lines->push(line);
    lines->push(line2);

    int count = 0;
    //mesh->m_tris->push_back(Tri(mesh_ind + line.IndA, mesh_ind + line.IndB, -1));

    while(!lines->isEmpty()){
        line = lines->pop();
        if(isUniqueLine[line.IndA * num_points + line.IndB]){
            isUniqueLine[line.IndA * num_points + line.IndB] = false;
            bestVert = -1;


            AB = (line.B - line.A);
            do{
                bestVert++;
                C = points[bestVert];
                AC = (C - line.A);
                norm = AB.cross(AC);
            } while(bestVert == line.IndA || bestVert == line.IndB);


            double test;
            double dot2;
            //cout << "looking for new C" << endl;
            for(int k = 0; k < num_points; k++){
                 if(k != line.IndA && k != line.IndB){
                    disp = (points[k] - line.A);
                    dot2 = disp.dot(norm);
                    test = ((line.A - points[k])).dot(norm) / norm.dot(norm);
                    //cout << k << ": " << test << endl;
                    //if(test * dot2 > 0.0) cout << test << " and " << dot2 << endl;
                    if(test < 0.0){
                        bestVert = k;
                        C = points[k];
                        AC = disp;
                        norm = AB.cross(AC);
                    }
                }
            }

            //cout << "Took: " << bestVert << endl;

            if(true){

                Line LCB, LAC, LBA;

                LCB.A     = C;
                LCB.B     = line.B;
                LCB.RA    = list_id[bestVert];
                LCB.RB    = line.RB;
                LCB.IndA  = bestVert;
                LCB.IndB  = line.IndB;

                LAC.A    = line.A;
                LAC.B    = C;
                LAC.RA   = line.RA;
                LAC.RB   = list_id[bestVert];
                LAC.IndA = line.IndA;
                LAC.IndB = bestVert;

                LBA.A    = line.B;
                LBA.B    = line.A;
                LBA.IndA = line.IndB;
                LBA.IndB = line.IndA;
                LBA.RA   = line.RB;
                LBA.RB   = line.RA;

                ind = LCB.IndA * num_points + LCB.IndB;
                if(isUniqueLine[ind])
                    lines->push(LCB);
                //isUniqueLine[ind] = false;

                ind = LAC.IndA * num_points + LAC.IndB;
                if(isUniqueLine[ind])
                    lines->push(LAC);
                //isUniqueLine[ind] = false;

                ind = LBA.IndA * num_points + LBA.IndB;
                if(isUniqueLine[ind])
                    lines->push(LBA);
               // isUniqueLine[ind] = false;


                if((line.RA != list_id[bestVert] || line.RB != list_id[bestVert])){
                    isUniqueLine[bestVert  * num_points + line.IndA]  = false;
                    isUniqueLine[line.IndB * num_points + bestVert]  = false;

                    mesh->m_tris->push_back(Tri(mesh_ind + line.IndA,
                                                mesh_ind + line.IndB,
                                                mesh_ind + bestVert));//mesh_ind + startInd + (((ind - startInd) + 1) % ring->points->size())));

                    norm.normalize();

                    norms[line.IndA] += norm;
                    norms[line.IndB] += norm;
                    norms[bestVert ] += norm;

                    num_norms[line.IndA]++;
                    num_norms[line.IndB]++;
                    num_norms[bestVert ]++;
                    count++;
                }
           }
        }
    }

    mesh->num_tris.push_back(count);
    for(int i = 0; i < num_points; i++)
        mesh->m_norms->push_back((norms[i] / (double) num_norms[i]).getNormalized());


    delete lines;

}

void buildMeshRecursivelyFromRoot(TreeMesh* mesh, Bud* bud){

    ControlRing r1, r2;
    ControlRing s;
    //bud->diameter = 10.f;
    //bud->end = bud->start + double3(30.f, 30.f, 30.f);
    makeControlRing(bud, bud->diameter * .5f, bud->start, bud->end, bud->phi, bud->theta, .0, s);

    ControlRing e;
    makeControlRing(bud, bud->diameter * .45f, bud->end, bud->start, bud->phi, bud->theta, .0, e);

    bool add_joint = false;
    bool useCollar = false;

    if(bud->aux){
        add_joint = true;
        makeControlRing(bud, bud->aux->diameter * .5f, bud->aux->start, bud->aux->end, bud->aux->phi, bud->aux->theta, .0, r1);
    }
    if(bud->term){
        add_joint = true;
        makeControlRing(bud, bud->term->diameter * .5f, bud->term->start, bud->term->end, bud->term->phi, bud->term->theta, .0, r2);
    }


    resolveOverlaps(s, e, r1, r2, useCollar);
    correctForCenter(s);
    correctForCenter(e);
    correctForCenter(r2);
    correctForCenter(r1);

    std::vector<ControlRing>* list = new std::vector<ControlRing>();

   //

    if(add_joint){
        if(useCollar) list->push_back(e);
        else list->push_back(s);
        list->push_back(r2);
        list->push_back(r1);  
    }
    else{
        list->push_back(s);
        list->push_back(e);
    }
    addMultipleRingsToMesh(list, mesh);



    if(useCollar){
        while(list->size() > 0)
            list->pop_back();
        list->push_back(s);
        list->push_back(e);
        addMultipleRingsToMesh(list, mesh);
    }

    delete s.points;
    delete s.tangents;
    delete e.points;
    delete e.tangents;
    delete list;

    if(bud->aux)
        buildMeshRecursively(mesh, bud->aux, r1);
    else delete r1.points;
    if(bud->term)
        buildMeshRecursively(mesh, bud->term, r2);
    else delete r2.points;




}

void buildMeshRecursively(TreeMesh* mesh, Bud* bud, ControlRing& s){

    ControlRing r1, r2;
    ControlRing e;

    bool add_joint = false;
    bool useCollar = false;


    if(bud->aux){
        add_joint = true;
        makeControlRing(bud, bud->aux->diameter * .5f, bud->aux->start, bud->aux->end, bud->aux->phi, bud->aux->theta, .0, r1);
    }
    else if(bud->aux_fate != ABORT){
        makeLeafs2(bud, mesh);
    }
    if(bud->term){
        makeControlRing(bud, bud->term->diameter * .5f, bud->term->start, bud->term->end, bud->term->phi, bud->term->theta, .0, r2);
        add_joint = true;
    }
    makeControlRing(bud, bud->diameter * .45f, bud->end, bud->start, bud->phi, bud->theta, .1, e);
    resolveOverlaps(s, e, r1, r2, useCollar);
    correctForCenter(s);
    correctForCenter(e);
    correctForCenter(r1);
    correctForCenter(r2);

    std::vector<ControlRing>* list = new std::vector<ControlRing>();

    if(add_joint){
        if(useCollar) list->push_back(e);
        else list->push_back(s);
        list->push_back(r2);
        list->push_back(r1);
        //addMultipleRingsToMesh(list, mesh);
    }
    else{
        list->push_back(s);
        list->push_back(e);
    }
    addMultipleRingsToMesh(list, mesh);

    if(useCollar){
        while(list->size() > 0)
            list->pop_back();
        list->push_back(s);
        list->push_back(e);
        addMultipleRingsToMesh(list, mesh);
    }
    delete s.points;
    delete s.tangents;
    delete e.points;
    delete e.tangents;
    delete list;

    if(bud->aux){
        buildMeshRecursively(mesh, bud->aux, r1);
    }
    else delete r1.points;
    if(bud->term){
        buildMeshRecursively(mesh, bud->term, r2);
    }
    else delete r2.points;




}

void makeControlRing(Bud * bud, double rad, float3 &center, float3 &end, double phi, double theta, double offThet, ControlRing &ring){

    const double NUM_SLICE = (double) VERTS_PER_RING;
    double3 p;
    double3 t;
    double ang;// ang2;
    ring.normal = (double3(center - end)).getNormalized();
    ring.center = double3(center);
    ring.radius = rad;
    ring.associated_bud = bud;

    const double ang2 = (double)phi + .5 * M_PI;

    double proj;

    const double r2 = rad * rad;

    for(double i = 0.f; i < NUM_SLICE; i += 1.f){
        ang = (2.0 * M_PI * i / NUM_SLICE);

        p.y  = -rad * cos(ang + offThet) * cos(ang2);
        proj = sqrt(r2 - p.y * p.y);

        p.x = proj * cos(ang + theta);
        p.z = proj * -sin(ang + theta);

        p += center;
        double3 ctop = (p-center).getNormalized();
        if (center.y == 0){
            t = (-ring.normal.cross(ctop)).getNormalized();
        }
        else{
           t = (ring.normal.cross(ctop)).getNormalized();
        }
        ring.points->push_back(p);
                ring.tangents->push_back(t);
    }

}

void resolveOverlaps(ControlRing &start, ControlRing &end, ControlRing& auxRing, ControlRing &termRing, bool &useCollar){

    auxRing.normal  = -auxRing.normal;
    termRing.normal = -termRing.normal;
    start.normal    = -start.normal;
    end.normal      = -end.normal;

    if(auxRing.points->size() > 0 && termRing.points->size() > 0){


        double3 startnorm    = (auxRing.normal - ((auxRing.normal.dot(start.normal) * start.normal))).getNormalized();
        double3 pointOnStart = startnorm * start.radius + start.center;

        double3 auxnorm      = (start.normal - ((start.normal.dot(auxRing.normal) * auxRing.normal))).getNormalized();
        double3 pointOnAux   = auxnorm * auxRing.radius + auxRing.center;

        double endD          = ((pointOnAux - pointOnStart).dot(auxnorm) / start.normal.dot(auxnorm));

        startnorm    = (termRing.normal - ((termRing.normal.dot(start.normal) * start.normal))).getNormalized();
        pointOnStart = startnorm * start.radius + start.center;


        double3 termnorm    = (start.normal - ((start.normal.dot(termRing.normal) * termRing.normal))).getNormalized();
        double3 pointOnTerm = termnorm * termRing.radius + termRing.center;

        endD = min(endD, ((pointOnTerm - pointOnStart).dot(termnorm) / start.normal.dot(termnorm)));

        double3  disp = (start.center + (start.normal * (endD - ERROR_OFF))) - end.center;

        if(disp.getMagnitude2() < (start.center - end.center).getMagnitude2()){
            useCollar = true;
            for(int i = 0; i < end.points->size(); i++)
                end.points->at(i) -= disp;
        }
        else useCollar = false;


        //The following moves the aux and term nodes away from the trunk

        termnorm    = (auxRing.normal - ((auxRing.normal.dot(termRing.normal) * termRing.normal))).getNormalized();
        pointOnTerm = termnorm * termRing.radius + termRing.center;

        auxnorm            = (termRing.normal - ((termRing.normal.dot(auxRing.normal) * auxRing.normal))).getNormalized();
        pointOnAux         = auxnorm * auxRing.radius + auxRing.center;

        //dir                = (pointOnTerm - pointOnEnd).getNormalized();

        disp        = ((pointOnTerm - pointOnAux).dot(termnorm) / auxRing.normal.dot(termnorm)) * auxRing.normal;
        double3 intersect   = disp + pointOnAux;

        disp *= (1.f + ERROR_OFF);

        for(int i = 0; i < auxRing.points->size(); i++)
            auxRing.points->at(i) += disp;


        disp = ((intersect - termRing.center).dot(termRing.normal) + ERROR_OFF) * termRing.normal;
        for(int i = 0; i < termRing.points->size(); i++)
            termRing.points->at(i) += disp;


    }
    else if(auxRing.points->size() > 0){
        double3 dir         = (end.normal - ((end.normal.dot(auxRing.normal) * auxRing.normal))).getNormalized();
        double3 pointOnAux  = dir * auxRing.radius + auxRing.center;
        double d;
        for(int i = 0; i < end.points->size(); i++){
           d = (pointOnAux - end.points->at(i)).dot(end.normal) / (end.normal.dot(end.normal)) + ERROR_OFF;
           end.points->at(i) += d * end.normal;
        }
        useCollar = false;
    }
    else if(termRing.points->size() > 0){
        double3 dir         = (end.normal - ((end.normal.dot(termRing.normal) * termRing.normal))).getNormalized();
        double3 pointOnTerm  = dir * termRing.radius + termRing.center;
        double d;
        for(int i = 0; i < end.points->size(); i++){
           d = (pointOnTerm - end.points->at(i)).dot(end.normal) / (end.normal.dot(end.normal)) + ERROR_OFF;
           end.points->at(i) += d * end.normal;
        }
        useCollar = false;
    }

    auxRing.normal  = -auxRing.normal;
    termRing.normal = -termRing.normal;
    start.normal    = -start.normal;
    end.normal      = -end.normal;


}

void correctForCenter(ControlRing& auxRing){
    auxRing.center = double3(0.f,0.f,0.f);
    for(int i = 0; i < auxRing.points->size(); i++){
        auxRing.center += auxRing.points->at(i);
    }
    auxRing.center /= (double) auxRing.points->size();
}

void correctForCenter(ControlDisc& auxRing){
    auxRing.plane.point = double3(0.f,0.f,0.f);
    for(int i = 0; i < auxRing.points->size(); i++){
        auxRing.plane.point += auxRing.points->at(i);
    }
    auxRing.plane.point /= (double) auxRing.points->size();
}


void makeLeafs2(Bud *bud, TreeMesh *mesh){
    int num_stems = (rand() % (MAX_STEMS_BRANCH - MIN_STEMS_BRANCH)) + MIN_STEMS_BRANCH;

    double   sphi, stheta, slen, phi, theta, disp, leaf_size, proj, theta2, phi2;
    double3  mid, p, tmp, sdir, perp1, perp2, cylDisp;
    Leaf leaf;


    double3 points[4];
    points[0] = double3( -.5, -.5, 0);
    points[1] = double3( 0.5, -.5, 0);
    points[2] = double3( 0.5, 0.5, 0);
    points[3] = double3( -.5, 0.5, 0);

    //cout<<"--------------------NEW BUD-------------------------"<<endl;
    //cout<<"bud end: "<<bud->end<<endl;
    //num_stems = 1;
    for(int k = 0; k<num_stems; k++){

        sphi   = randf() * (MAX_PHI - MIN_PHI) + MIN_PHI;
        double ttheta = randf() * M_PI;
        //random direction with ttheta about y-axis
        sdir =float3(sin(sphi)*cos(ttheta),
                    cos(sphi),
                    sin(sphi)*sin(ttheta));
        //now rotate using phi and theta of direction:
        double xp = sdir.x * cos(bud->phi) - sdir.y * sin(bud->phi);
        double yp = sdir.x * sin(bud->phi) + sdir.y * cos(bud->phi);
        double zp = sdir.z;
        sdir.x = zp*sin(bud->theta) + xp * cos(bud->theta);
        sdir.y = yp;
        sdir.z = zp * cos(bud->theta) - xp*sin(bud->theta);
        sdir.normalize();
        slen  =  /**min(randf(),MIN_STEM_RATIO) **/ bud->length;





        int num_leaves = ((rand() % (MAX_LEAF_STEM - MIN_LEAF_STEM)) + MIN_LEAF_STEM);
        //num_leaves = 10;
        for(int i =0; i<num_leaves; i++){
           leaf_size = randf() * (MAX_LEAF_SIZE - MIN_LEAF_SIZE) + MIN_LEAF_SIZE;
           leaf_size /= 2.0;
           if(abs(sdir.x) < 0.01 && abs(sdir.z) < 0.01){
               perp1 = double3(1,0,0);
               perp2 = double3(0,0,1);
           }
           else{
               perp1 = sdir.cross(float3(sdir.x,sdir.y,0));
               perp1.normalize();

               perp2 = sdir.cross(perp1);
               perp2.normalize();
           }
           cylDisp = (randf()-0.5) * ((MAX_DIST_STEM - MAX_DIST_STEM) + MIN_DIST_STEM)* perp1
                     + (randf()-0.5) * ((MAX_DIST_STEM - MAX_DIST_STEM) + MIN_DIST_STEM)* perp2;
           cylDisp.normalize();
           cylDisp += randf()/2.0 * sdir;
           cylDisp.normalize();

           disp  = randf() * slen; //Gets info for general position relative to end of limb

           mid = bud->end;
           mid += disp*sdir + (randf() +0.5) * 2.0 *cylDisp;


           if(randf()< 0.5){
               leaf.bl = mid + -leaf_size * cylDisp + -leaf_size * sdir;
               leaf.br = mid + -leaf_size * cylDisp + leaf_size * sdir;
               leaf.tr = mid + leaf_size * cylDisp + leaf_size * sdir;
               leaf.tl = mid + leaf_size * cylDisp + -leaf_size * sdir;
           }
           else{
               leaf.bl = mid + -leaf_size * cylDisp + -leaf_size * sdir;
               leaf.tr = mid + leaf_size * cylDisp + leaf_size * sdir;
               leaf.tl = mid + leaf_size * cylDisp + -leaf_size * sdir;
               leaf.br = mid + -leaf_size * cylDisp + leaf_size * sdir;
           }



            phi   = randf() * (MAX_PHI - MIN_PHI) + MIN_PHI;
            theta = randf() * 2.0 * M_PI;


//            mid.y = -disp * cos(theta) * cos(phi);
//            proj  = sqrt(disp * disp - mid.y * mid.y);
//            mid.x = proj *  cos(theta);
//            mid.z = proj * -sin(theta);

//            mid  += bud->end;

//            phi   = randf() * M_PI;
//            theta = randf() * 2.0 * M_PI;
//            for(int j = 0; j < 4; j++){
//                tmp    = points[j] * leaf_size;
//                theta2 = theta + atan2(-tmp.z, tmp.x);
//                disp  = tmp.getMagnitude();
//                phi2  = phi + acos(tmp.y / disp);
//                p.y   = disp * cos(phi2);
//                p.x   = disp *  cos(theta2) * sin(phi2);
//                p.z   = disp *  sin(theta2) * sin(phi2);

//                //for testing:
//               // p = float3(0,0,0);
//                switch(j){
//                    case 0:
//                        leaf.bl = p + mid;
//                        break;
//                    case 1:
//                        leaf.br = p + mid;
//                        break;
//                    case 2:
//                        leaf.tr = p + mid;
//                        break;
//                    default:
//                        leaf.tl = p + mid;
//                        break;

//                }
//            }
            leaf.normal = (leaf.br - leaf.bl).cross(leaf.tl - leaf.bl).getNormalized();
            if(leaf.normal.y < 0)
                leaf.normal = -leaf.normal;

           /* cout << "dot bl: " << (leaf.tl - leaf.bl).dot(leaf.br - leaf.bl) << endl;
            c*///out << "dot tr: " << (leaf.tl - leaf.tr).dot(leaf.br - leaf.tr) << endl;
           // cout << "LEAF: " << leaf.bl << "\n" << leaf.br << "\n" << leaf.tr << "\n"  << leaf.tl << "\n" << leaf.normal << endl;
            mesh->m_leafs[rand() % NUM_LEAF_COLORS]->push_back(leaf);



        }

    }
}


void makeLeafs(Bud* bud, TreeMesh* mesh){
    int num_leafs = (rand() % (MAX_LEAF_PER_BRANCH - MIN_LEAF_PER_BRANCH)) + MIN_LEAF_PER_BRANCH;


    double   phi, theta, disp, leaf_size, proj, theta2, phi2;
    double3  mid, p, tmp;
    Leaf leaf;


    double3 points[4];
    points[0] = double3( -.5, -.5, 0);
    points[1] = double3( 0.5, -.5, 0);
    points[2] = double3( 0.5, 0.5, 0);
    points[3] = double3( -.5, 0.5, 0);

    for(int i = 0; i < num_leafs; i++){
        phi   = randf() * (MAX_PHI - MIN_PHI) + MIN_PHI;
        theta = randf() * 2.0 * M_PI;
        disp  = randf() * bud->length; //Gets info for general position relative to end of limb

        leaf_size = randf() * (MAX_LEAF_SIZE - MIN_LEAF_SIZE) + MIN_LEAF_SIZE;


        mid.y = -disp * cos(theta) * cos(phi);
        proj  = sqrt(disp * disp - mid.y * mid.y);
        mid.x = proj *  cos(theta);
        mid.z = proj * -sin(theta);

        mid  += bud->end;

        phi   = randf() * M_PI;
        theta = randf() * 2.0 * M_PI;
        for(int j = 0; j < 4; j++){
            tmp    = points[j] * leaf_size;
            theta2 = theta + atan2(-tmp.z, tmp.x);
            disp  = tmp.getMagnitude();
            phi2  = phi + acos(tmp.y / disp);
            p.y   = disp * cos(phi2);
            p.x   = disp *  cos(theta2) * sin(phi2);
            p.z   = disp *  sin(theta2) * sin(phi2);
//            cout << tmp << endl;
//            cout <<"phi: "<< phi << " theta: " << theta << endl;
//            cout << "point "<<j<<": "<< p<< endl;

            switch(j){
                case 0:
                    leaf.bl = p + mid;
                    break;
                case 1:
                    leaf.br = p + mid;
                    break;
                case 2:
                    leaf.tr = p + mid;
                    break;
                default:
                    leaf.tl = p + mid;
                    break;

            }
        }
        leaf.normal = (leaf.br - leaf.bl).cross(leaf.tl - leaf.bl).getNormalized();
        if(leaf.normal.y < 0)
            leaf.normal = -leaf.normal;

       /* cout << "dot bl: " << (leaf.tl - leaf.bl).dot(leaf.br - leaf.bl) << endl;
        c*///out << "dot tr: " << (leaf.tl - leaf.tr).dot(leaf.br - leaf.tr) << endl;
       // cout << "LEAF: " << leaf.bl << "\n" << leaf.br << "\n" << leaf.tr << "\n"  << leaf.tl << "\n" << leaf.normal << endl;
        mesh->m_leafs[rand() % NUM_LEAF_COLORS]->push_back(leaf);



    }
}

void build(TreeMesh* mesh, Bud* bud){
    ControlDisc start(bud, double3(bud->start), double3(bud->end - bud->start).getNormalized(), bud->diameter * .5f);
    buildMeshRecursivelyTwo(mesh, start);
}

void buildMeshRecursivelyTwo(TreeMesh* mesh, ControlDisc &start){
    QList<ControlDisc> list;
    QList<ControlDisc> resolvedNodes;
    vector<ControlDisc> hull;
    hull.push_back(start);

    if(start.bud->aux){
        list.push_back(ControlDisc(start.bud->aux, double3(start.bud->aux->start), double3(start.bud->aux->end - start.bud->aux->start).getNormalized(), start.bud->aux->diameter * .5f));
    }

    if(start.bud->term){
        list.push_back(ControlDisc(start.bud->term, double3(start.bud->term->start), double3(start.bud->term->end - start.bud->term->start).getNormalized(), start.bud->term->diameter * .5f));
    }

    ControlDisc discOne, discTwo, selected, transDisc1, transDisc2;
    double3 norm1, norm2, point1, point2, intersect, disp;

    int minInd;

    int size = list.size();

    double d, tmp;

    if(size > 1){
        while(size > 1){
            double t_vals[size];
            bool   used[size];
            double3 close_points[size];
            double3 far_points[size];

            for(int i = 0; i < size; i++){
                discOne   = list.at(i);
                used[i]   = false;
                t_vals[i] = 0; //Never moves backwards

    //            norm1     = (discOne.normal - ((discOne.plane.normal.dot(start.plane.normal) * start.plane.normal))).getNormalized();
    //            point1    = norm1 * start.radius + start.plane.point;

                norm2     = (start.plane.normal - ((start.plane.normal.dot(discOne.plane.normal) * discOne.plane.normal))).getNormalized();
                //point2    = norm2 * discOne.radius + discOne.plane.point;

                close_points[i] = norm2 * discOne.radius + discOne.plane.point;
                far_points[i]   = -norm2 * discOne.radius + discOne.plane.point;
                //double endD          = ((point2 - point1).dot(norm2) / start.normal.dot(norm2));

            }

            for(int i = 0; i < size - 1; i++){
                discOne = list.at(i);
                for(int j = i + 1; j < size; j++){  //Calculates the displacement value for each branch such that no matter what, it intersects no other branch
                    discTwo = list.at(j);

                    norm1    = (discTwo.plane.normal - ((discTwo.plane.normal.dot(discOne.plane.normal) * discOne.plane.normal))).getNormalized();
                    point1   = norm1 * discOne.radius + discOne.plane.point;

                    norm2    = (discOne.plane.normal - ((discOne.plane.normal.dot(discTwo.plane.normal) * discTwo.plane.normal))).getNormalized();
                    point2   = norm2 * discTwo.radius + discTwo.plane.point;


                    d           = ((point1 - point2).dot(norm1) / discTwo.plane.normal.dot(norm1));
                    transDisc2  = discTwo;

                    transDisc2.plane.point += d * discTwo.plane.normal;

                    intersect   = d * discTwo.plane.normal + point2;
                    t_vals[j]   = max(t_vals[j], d);

                    d = ((intersect - discOne.plane.point).dot(discOne.plane.normal));

                    transDisc1 = discOne;
                    transDisc1.plane.point += d * discOne.plane.normal;


                    t_vals[i]   = max(t_vals[i], d); //Gets displacement required to avoid intersections
                }
            }

            point1 = (far_points[0] + t_vals[0] * list.at(0).plane.normal);
            d = distToPlane(point1, -start.plane.normal, start.plane);
            minInd = 0;
            for(int i = 1; i < size; i++){
                point1 = (far_points[i] + t_vals[i] * list.at(i).plane.normal);
                tmp    = distToPlane(point1, -start.plane.normal, start.plane);
                if(tmp < d){
                    d = tmp;
                    minInd = i;
                }
            }

            selected = list.at(minInd);//list.at(minInd);
            size = list.size();

            if(t_vals[minInd] > selected.bud->length){
                //list.removeAt(minInd);
                //delete selected
                cout << "bud too long" << endl;
                list.removeAt(minInd);
                //We can add these because by definition the starting discs of these must be part of the joint
                if(selected.bud->aux){
                    list.push_back(ControlDisc(selected.bud->aux, double3(selected.bud->aux->start), double3(selected.bud->aux->end - selected.bud->aux->start).getNormalized(), selected.bud->aux->diameter * .5f));
                }

                if(selected.bud->term){
                    list.push_back(ControlDisc(selected.bud->term, double3(selected.bud->term->start), double3(selected.bud->term->end - selected.bud->term->start).getNormalized(), selected.bud->term->diameter * .5f));
                }
            }
            else{ //Add make sub-joint

                if(size > 2)
                    cout << "crazy joint alert!!!!" << endl;

                disp   = t_vals[minInd] * list.at(minInd).plane.normal;
                point1 = far_points[minInd] + t_vals[minInd] * list.at(minInd).plane.normal;

                selected.plane.point += disp; //Translates disc for drawing
                for(int i = 0; i < selected.points->size(); i++){
                    selected.points->at(i) += disp;
                }

//resolvedNodes.push_back(selected);


                //get closest point to base that is still part of joint, then build plane between that point and the point on the ring ending the joint
//                point2 = (close_points[0] + t_vals[0] * list.at(0).plane.normal);
//                d = distToPlane(point2, -start.plane.normal, start.plane);
//                minInd = 0;
//                for(int i = 1; i < size; i++){
//                    point2 = (close_points[i] + t_vals[i] * list.at(i).plane.normal);
//                    tmp    = distToPlane(point2, -start.plane.normal, start.plane);
//                    if(tmp < d){
//                        d = tmp;
//                        minInd = i;
//                    }
//                }
//                point2 = (close_points[minInd] + t_vals[minInd] * list.at(minInd).plane.normal);

                ControlDisc discus;
                for(int i = 0; i < list.size(); i++){
                    if(i != minInd){
                        discus = ControlDisc((list.at(i)));

                        disp = t_vals[i] * discus.plane.normal;
                        discus.plane.point += disp;
                        for(int j = 0; j < discus.points->size(); j++){
                            discus.points->at(j) += disp;
                        }
                        list.replace(i, discus);
                        hull.push_back(discus);
                    }
                }
                hull.push_back(selected);
                convexHull(&hull, mesh);

                while(!list.isEmpty())
                    resolvedNodes.push_back(list.takeFirst());
            }

            size = list.size();
        }
    }
    else if(list.size() == 1) {
        hull.push_back(list.at(0));
        convexHull(&hull, mesh);
        resolvedNodes.push_back(list.at(0));
    }
    else{ //No children use end
        hull.push_back(ControlDisc(start.bud, double3(start.bud->end), double3(start.bud->start - start.bud->end).getNormalized(), start.bud->diameter * .5f));
        convexHull(&hull, mesh);
    }

    ControlDisc c;
    for(int i = 0; i < resolvedNodes.size(); i++){
        c = resolvedNodes.at(i);
        buildMeshRecursivelyTwo(mesh, c);
    }

}



